
#include <stream.h>
#include <source.h>
#include <panic.h>
#include <stdlib.h>
#include <i2c.h>
#include <ps.h>
#include <led.h>
#include <pio.h>
#include <chain.h>
#include <operator.h>
#include "macros.h"
#include "adi1787.h"

AncAdaptiveTaskData anc_adaptive_tast;
uint16 ADI1787_FFL_LOW_VALUE_REG = 0x21B4;
uint16 ADI1787_FFL_MID_VALUE_REG = 0x21AC;
uint16 ADI1787_FFL_HIGH_VALUE_REG = 0x21A4;
uint16 ADI1787_FFR_LOW_VALUE_REG = 0x219C;
uint16 ADI1787_FFR_MID_VALUE_REG = 0x2194;
uint16 ADI1787_FFR_HIGH_VALUE_REG = 0x218C;

#define env_ident_detect_times 50     /* 检测次数 */
#define env_ident_detect_interval 100 /* 检测间隔ms */
#define env_ident_detect_asset 40     /* 识别准确率判定 */
uint16 current_env_ident_index = 0;
uint16 anc_adaptive_enabled = 0;
uint8 anc_on_state = AncOnStateHigh;
EnvironmentValueData noise_level_low = {0x0008, 0x0006, 0x0001};       /* 安静场景 低频-54dB*/
EnvironmentValueData noise_level_middle = {0x0020, 0x000a, 0x0003};    /* 办公场景 低频-42dB*/
EnvironmentValueData noise_level_high = {0x0080, 0x0010, 0x0005};      /* 通勤场景 低频-30dB*/
EnvironmentValueData noise_level_weighting = {0x0005, 0x0003, 0x0002}; /* 环境识别中低、中、高噪声频率计权占比 */
uint8 noise_level_low_count = 0;
uint8 noise_level_middle_count = 0;
uint8 noise_level_high_count = 0;

static void My_ANC_Adaptive_Event_Handler(Task task, MessageId id, Message msg);
static EnvironmentValueData Get_FFmic_Crossover_Value(void);
static void Environment_Detect_Form_FFmic(void);
static void Environment_Identification_Result_count(void);
////////////////////////////////////////////////////////////////////////////////////////////////////////////
static EnvironmentValueData Get_FFmic_Crossover_Value(void)
{
    uint8 ret_l[4], ret_m[4], ret_h[4];
    EnvironmentValueData env_level_l;
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_FFL_LOW_VALUE_REG, 4, ret_l);
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_FFL_MID_VALUE_REG, 4, ret_m);
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_FFL_HIGH_VALUE_REG, 4, ret_h);
    env_level_l.adc_value_l = ((uint16)ret_l[1] << 8) + ret_l[2];
    env_level_l.adc_value_m = ((uint16)ret_m[1] << 8) + ret_m[2];
    env_level_l.adc_value_h = ((uint16)ret_h[1] << 8) + ret_h[2];
    // ANC_ADAPTIVE_DEBUG_INFO(("FFL_L [%d] [%d] FFL_M [%d] [%d] FFL_H [%d] [%d]\n", ret_l[1], ret_l[2], ret_m[1], ret_m[2], ret_h[1], ret_h[2]));
    // ANC_ADAPTIVE_DEBUG_RAW(("FFL_L [%d] FFL_M [%d] FFL_H [%d]\n", env_level_l.adc_value_l, env_level_l.adc_value_m, env_level_l.adc_value_h));
    return env_level_l;
}

static void Environment_Detect_Form_FFmic(void)
{
    EnvironmentValueData env_level_l;
    int compared_0, compared_1, compared_2;
    env_level_l = Get_FFmic_Crossover_Value();
    compared_0 = ABS(env_level_l.adc_value_l - noise_level_low.adc_value_l) * noise_level_weighting.adc_value_l +
                 ABS(env_level_l.adc_value_m - noise_level_low.adc_value_m) * noise_level_weighting.adc_value_m +
                 ABS(env_level_l.adc_value_h - noise_level_low.adc_value_h) * noise_level_weighting.adc_value_h;
    compared_1 = ABS(env_level_l.adc_value_l - noise_level_middle.adc_value_l) * noise_level_weighting.adc_value_l +
                 ABS(env_level_l.adc_value_m - noise_level_middle.adc_value_m) * noise_level_weighting.adc_value_m +
                 ABS(env_level_l.adc_value_h - noise_level_middle.adc_value_h) * noise_level_weighting.adc_value_h;
    compared_2 = ABS(env_level_l.adc_value_l - noise_level_high.adc_value_l) * noise_level_weighting.adc_value_l +
                 ABS(env_level_l.adc_value_m - noise_level_high.adc_value_m) * noise_level_weighting.adc_value_m +
                 ABS(env_level_l.adc_value_h - noise_level_high.adc_value_h) * noise_level_weighting.adc_value_h;
    ANC_ADAPTIVE_DEBUG_RAW(("compared_0 [%d] compared_1 [%d] compared_2 [%d]\n", compared_0, compared_1, compared_2));

    if ((compared_0 < compared_1) && (compared_0 < compared_2))
    {
        noise_level_low_count++; /* noise_level_low */
    }
    else if ((compared_2 < compared_0) && (compared_2 < compared_1))
    {
        noise_level_high_count++; /* noise_level_high */
    }
    else
    {
        noise_level_middle_count++; /* noise_level_middle */
    }

    if (current_env_ident_index >= (env_ident_detect_times - 1))
    {
        MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveEnvId, 0, 0);
    }
    else
    {
        current_env_ident_index++;
    }
}

static void Environment_Identification_Result_count(void)  /* 统计 检查次数*检查间隔(目前5秒) 内噪声识别结果，符合判断标准进行场景切换 */
{
   uint8 response[3] = {0x0b, 0x0b, 0x00}; /* GAIA发送ANC_ON实时状态*/
    if (noise_level_low_count >= env_ident_detect_asset) /* noise_level_low */
    {
        ANC_ADAPTIVE_DEBUG_RAW(("noise_level_low\n"));
        MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveSetLevelLow, 0, 0);

        response[2] = (uint8)(AncOnStateLow | (anc_adaptive_enabled << 4) | (ANC_ADAPTIVE_SUPPORTED << 7));
        GaiaSendPacket(gaia_get_transport(), GAIA_VENDOR_QTIL, GAIA_EVENT_NOTIFICATION, GAIA_STATUS_NONE, 3, response);
    }
    else if (noise_level_middle_count >= env_ident_detect_asset) /* noise_level_middle */
    {
        ANC_ADAPTIVE_DEBUG_RAW(("noise_level_middle\n"));
        MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveSetLevelMid, 0, 0);

        response[2] = (uint8)(AncOnStateMid | (anc_adaptive_enabled << 4) | (ANC_ADAPTIVE_SUPPORTED << 7));
        GaiaSendPacket(gaia_get_transport(), GAIA_VENDOR_QTIL, GAIA_EVENT_NOTIFICATION, GAIA_STATUS_NONE, 3, response);
    }
    else if (noise_level_high_count >= env_ident_detect_asset) /* noise_level_high */
    {
        ANC_ADAPTIVE_DEBUG_RAW(("noise_level_high\n"));
        MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveSetLevelHigh, 0, 0);

        response[2] = (uint8)(AncOnStateHigh | (anc_adaptive_enabled << 4) | (ANC_ADAPTIVE_SUPPORTED << 7));
        GaiaSendPacket(gaia_get_transport(), GAIA_VENDOR_QTIL, GAIA_EVENT_NOTIFICATION, GAIA_STATUS_NONE, 3, response);
    }
    else /* 识别不准确，不调整状态 */
    {
        ANC_ADAPTIVE_DEBUG_RAW(("noise_level_unknown\n"));
    }
    noise_level_low_count = 0;
    noise_level_middle_count = 0;
    noise_level_high_count = 0;
    current_env_ident_index = 0;
}

void Anc_Adaptive_Set_Level_High(void)
{
    uint8 Mixer1_B0[4] = {0xE7, 0xA7, 0x8F, 0xD0};
    uint8 Mixer1_B1[4] = {0x08, 0x00, 0x00, 0x00};
    uint8 Mixer1_B2[4] = {0x08, 0x00, 0x00, 0x00};
    uint8 Mixer1_A1[4] = {0x04, 0x00, 0x00, 0x00};
    uint8 Mixer1_A2[4] = {0x00, 0x00, 0x00, 0x00};
    uint8 Mixer1_addr[1] = {0x34};
    uint8 Mixer2_B0[4] = {0xE1, 0xAF, 0x77, 0xD0};
    uint8 Mixer2_addr[1] = {0x2A};
    uint8 ret_l[1];
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, ret_l);
    if (ret_l[0] == BANK_A[0])  /* BANK A状态，需要safeload切换 */
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer1_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer1_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);

        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer2_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer2_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);
    }
    else
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xD3D0, 4, Mixer1_B2);   /* 确保mix FF增益正常 */
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xD3A8, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, BANK_A); //BANK A
    }

    anc_on_state = AncOnStateHigh;
}

void Anc_Adaptive_Set_Level_Mid(void)
{
    uint8 Mixer1_B0[4] = {0xE7, 0xA7, 0x8F, 0xD0};
    uint8 Mixer1_B1[4] = {0x08, 0x00, 0x00, 0x00};
    uint8 Mixer1_B2[4] = {0x08, 0x00, 0x00, 0x00};
    uint8 Mixer1_A1[4] = {0x04, 0x00, 0x00, 0x00};
    uint8 Mixer1_A2[4] = {0x00, 0x00, 0x00, 0x00};
    uint8 Mixer1_addr[1] = {0x34};
    uint8 Mixer2_B0[4] = {0xE1, 0xAF, 0x77, 0xD0};
    uint8 Mixer2_addr[1] = {0x2A};
    uint8 ret_l[1];
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, ret_l);
    if (ret_l[0] == BANK_C[0])  /* BANK C状态，需要safeload切换 */
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer1_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer1_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);

        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer2_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer2_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);
    }
    else
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xDDD0, 4, Mixer1_B2);   /* 确保mix FF增益正常 */
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xDDA8, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, BANK_C); //BANK C
    }

    anc_on_state = AncOnStateMid;
}

void Anc_Adaptive_Set_Level_Low(void)
{
    uint8 Mixer1_B0[4] = {0xE7, 0xA7, 0x8F, 0xD0};
    uint8 Mixer1_B1[4] = {0x08, 0x00, 0x00, 0x00};
    uint8 Mixer1_B2[4] = {0x00, 0x10, 0x00, 0x00};
    uint8 Mixer1_A1[4] = {0x04, 0x00, 0x00, 0x00};
    uint8 Mixer1_A2[4] = {0x00, 0x00, 0x00, 0x00};
    uint8 Mixer1_addr[1] = {0x34};
    uint8 Mixer2_B0[4] = {0xE1, 0xAF, 0x77, 0xD0};
    uint8 Mixer2_addr[1] = {0x2A};
    uint8 ret_l[1];
    my_i2c_reg_read(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, ret_l);
    if (ret_l[0] == BANK_C[0])  /* BANK C状态，需要safeload切换 */
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer1_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer1_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);

        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06B, 4, Mixer2_B0);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06F, 4, Mixer1_B1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC073, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC077, 4, Mixer1_A1);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07B, 4, Mixer1_A2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC06A, 1, Mixer2_addr);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_01);
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xC07F, 1, VALUE_00);
    }
    else
    {
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xDDD0, 4, Mixer1_B2);   /* 确保mix FF增益较低 */
        ADI1787_Registers_write(i2c_addr_1787_8bit, 0xDDA8, 4, Mixer1_B2);
        ADI1787_Registers_write(i2c_addr_1787_8bit, ADI1787_BANK_CONTROL_REG, 1, BANK_C); //BANK C
    }
    anc_on_state = AncOnStateLow;
}

static void My_ANC_Adaptive_Event_Handler(Task task, MessageId id, Message msg)
{
    UNUSED(task);
    UNUSED(msg);
    if (anc_is_changing)
    {
        MessageSendLater(AncAdaptiveGetTask(), id, 0, 20);
    }else
    {
        switch (id)
        {
        case EventAncAdaptiveEnvDet:
            if (anc_adaptive_enabled)
            {
                Environment_Detect_Form_FFmic();
                MessageCancelAll(AncAdaptiveGetTask(), EventAncAdaptiveEnvDet);
                MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveEnvDet, 0, env_ident_detect_interval);
            }
            break;
        case EventAncAdaptiveEnvId:
            if (anc_adaptive_enabled)
            {
                Environment_Identification_Result_count();
            }
            break;
        case EventAncAdaptiveSetLevelHigh:
            Anc_Adaptive_Set_Level_High();
            ANC_ADAPTIVE_DEBUG_RAW(("command noise_level_high\n"));
            break;
        case EventAncAdaptiveSetLevelMid:
            Anc_Adaptive_Set_Level_Mid();
            ANC_ADAPTIVE_DEBUG_RAW(("command noise_level_mid\n"));
            break;
        case EventAncAdaptiveSetLevelLow:
            Anc_Adaptive_Set_Level_Low();
            ANC_ADAPTIVE_DEBUG_RAW(("command noise_level_low\n"));
            break;
        case EventAncAdaptiveStart:
            ANC_Adaptive_Start();
            ANC_ADAPTIVE_DEBUG_RAW(("EventAncAdaptiveStart\n"));
            break;
        case EventAncAdaptiveStop:
            ANC_Adaptive_Stop();
            ANC_ADAPTIVE_DEBUG_RAW(("EventAncAdaptiveStop\n"));
            break;
        default:
            break;
        }
    }
}

void ANC_Adaptive_Task_Init(void)
{
    memset(&anc_adaptive_tast, 0, sizeof(anc_adaptive_tast));
    anc_adaptive_tast.task.handler = My_ANC_Adaptive_Event_Handler;
    noise_level_low_count = 0;
    noise_level_middle_count = 0;
    noise_level_high_count = 0;
    current_env_ident_index = 0;
}

void ANC_Adaptive_Task_DeInit(void)
{
    uint16 requested_words;
    requested_words = PS_SIZE_ADJ(sizeof(uint16));
    PsStore(anc_adaptive_enabled_key, (const uint16 *)&anc_adaptive_enabled, requested_words);
    PsStore(anc_on_state_key, (const uint16 *)&anc_on_state, requested_words);
    ANC_Adaptive_Stop();
    memset(&anc_adaptive_tast, 0, sizeof(anc_adaptive_tast));
}

void ANC_Adaptive_Start(void)
{
    anc_adaptive_enabled = 1;
    noise_level_low_count = 0;
    noise_level_middle_count = 0;
    noise_level_high_count = 0;
    current_env_ident_index = 0;
    MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveEnvDet, 0, env_ident_detect_interval);
    MessageSendLater(AncAdaptiveGetTask(), EventAncAdaptiveEnvId, 0, env_ident_detect_interval * env_ident_detect_times);
}

void ANC_Adaptive_Stop(void)
{
    anc_adaptive_enabled = 0;
    MessageCancelAll(AncAdaptiveGetTask(), EventAncAdaptiveEnvDet);
    MessageCancelAll(AncAdaptiveGetTask(), EventAncAdaptiveEnvId);
}
